/*
    Copyright 2006 (c) by Aitor Viana Sanchez, Pablo Parra Espada
    University of Alcala,
    Computer Engineering Department.

    For further information, please visit http://atc1.aut.uah.es

    This software is provided under the terms of the GNU General Public v2
    Licence. A full copy of the GNU GPL is provided in the file COPYING
    found in the development root of ERCOS-RT.
*/
#include <public/config.h>

#include <asm.h>
#include <leon3/asm.h>

//#ifndef _SOFT_FLOAT
#ifndef CONFIG_HAS_FPU
#define PSR_INIT    0x000000c0	/* CPU traps off, set s and ps, and FP unit  */
#else
#define PSR_INIT    0x000010c0 /* CPU traps off, set s and ps */
#endif

#define MEMCFG1     0x000000ff	/* MEC config reg 1 (No PROM Access) */
#define MEMCFG2     0x82206000   /* MEC config reg 2 (SDRAM Configuration, 64 MBytes) */
#define MEMCFG3     0x00136000   /* MEC config reg 3 (SDRAM Refresh counter) */

#ifdef CONFIG_FAST_MODE
#define WIM_INIT    0x00000002	/* Initial invalid window value	*/
#else
#define WIM_INIT    0x000000FE	/* Initial invalid window value	*/
#endif

/* Initial stack pointer located at the end of the SDRAM (64 Mbytes) */
#define SPP_INIT    CONFIG_ERCOS_STACK_BEGIN_ADDRESS

#define UARTA_SCALER 	0x00000081 /* UARTA scaler reload value. Set to 38400 bauds */

//.section ".text"
.section ".romvec"
    .align 4
    .global ercos_hal_reset_entry

/*	Reset entry point	*/
ercos_hal_reset_entry:
trap_table:

    wr 	%g0, PSR_INIT, %psr	/* traps off, set s and ps	*/
    mov %g0, %tbr			/*	TBR at 0x00000000	*/
    b	system_reset		/*	branch to system reset	*/
    mov	%g0, %wim			/* 	Clears the window invalid register	*/

    TRAP_ENTRY(0x01)		/* 01 Instruction acess	*/
    TRAP_ENTRY(0x02)		/* 02 Ilegal instruction	*/
    TRAP_ENTRY(0x03)		/* 03 Priviledge instruction	*/

    TRAP(0x04, fpu_dis)		/* 04 Floating point disable	*/

    mov	%psr, %l0
    mov	%wim, %l3
    b	wof_trap			/*	05 Window overfow	*/
    nop

    mov	%psr, %l0
    mov	%wim, %l3
    b	wuf_trap			/*	06 Window underflow	*/
    nop

    TRAP_ENTRY(0x07)	/* 07 Memory access not aligment */
    TRAP_ENTRY(0x08)	/* 08 Flotating point exception */
    TRAP_ENTRY(0x09)	/* 09 Data access exception */
    TRAP_ENTRY(0x0a)	/* 10 Tag overflow */
    TRAP_ENTRY(0x0b)	/* 11 Not handled exception */
    TRAP_ENTRY(0x0c)	/* 12 Not handled exception */
    TRAP_ENTRY(0x0d)	/* 13 Not handled exception */
    TRAP_ENTRY(0x0e)	/* 14 Not handled exception */
    TRAP_ENTRY(0x0f)	/* 15 Not handled exception */
    TRAP_ENTRY(0x10)	/* 16 Not handled exception */
    TRAP_ENTRY(0x11)	/* 17 Masked hardware errors */
    TRAP_ENTRY(0x12)	/* 18 Ext INT0 */
    TRAP_ENTRY(0x13)	/* 19 Ext INT1 */
    TRAP_ENTRY(0x14)	/* 20 UART A data ready or trasnmitter ready */
    TRAP_ENTRY(0x15)	/* 21 UART B data ready or trasnmitter ready */
    TRAP_ENTRY(0x16)	/* 22 Correctable error in memory */
    TRAP_ENTRY(0x17)	/* 23 UART error */
    TRAP_ENTRY(0x18)	/* 24 DMA access error */
    TRAP_ENTRY(0x19)	/* 25 DMA time-out */
    TRAP_ENTRY(0x1a)	/* 26 EXT INT2 */
    TRAP_ENTRY(0x1b)	/* 27 EXT INT3 */
    TRAP_ENTRY(0x1c)	/* 28 General purpose timer */
    TRAP_ENTRY(0x1d)	/* 29 Real time clock */
    TRAP_ENTRY(0x1e)	/* 30 EXT INT4 (ethernet) */
    TRAP_ENTRY(0x1f)	/* 31 Watch dog time out */

  BAD_TRAP(0x20); BAD_TRAP(0x21); BAD_TRAP(0x22); BAD_TRAP(0x23);	! 20 - 23 undefined
  BAD_TRAP(0x24);                                     				! 24 cp_disabled
            BAD_TRAP(0x25); BAD_TRAP(0x26); BAD_TRAP(0x27);       	! 25 - 27 undefined
  BAD_TRAP(0x28);                                     				! 28 cp_exception
            BAD_TRAP(0x29); BAD_TRAP(0x2A); BAD_TRAP(0x2B);       	! 29 - 2B undefined
  BAD_TRAP(0x2C); BAD_TRAP(0x2D); BAD_TRAP(0x2E); BAD_TRAP(0x2F); 	! 2C - 2F undefined
  BAD_TRAP(0x30); BAD_TRAP(0x31); BAD_TRAP(0x32); BAD_TRAP(0x33); 	! 30 - 33 undefined
  BAD_TRAP(0x34); BAD_TRAP(0x35); BAD_TRAP(0x36); BAD_TRAP(0x37); 	! 34 - 37 undefined
  BAD_TRAP(0x38); BAD_TRAP(0x39); BAD_TRAP(0x3A); BAD_TRAP(0x3B); 	! 38 - 3B undefined
  BAD_TRAP(0x3C); BAD_TRAP(0x3D); BAD_TRAP(0x3E); BAD_TRAP(0x3F); 	! 3C - 3F undefined
  BAD_TRAP(0x40); BAD_TRAP(0x41); BAD_TRAP(0x42); BAD_TRAP(0x43); 	! 40 - 43 undefined
  BAD_TRAP(0x44); BAD_TRAP(0x45); BAD_TRAP(0x46); BAD_TRAP(0x47); 	! 44 - 47 undefined
  BAD_TRAP(0x48); BAD_TRAP(0x49); BAD_TRAP(0x4A); BAD_TRAP(0x4B); 	! 48 - 4B undefined
  BAD_TRAP(0x4C); BAD_TRAP(0x4D); BAD_TRAP(0x4E); BAD_TRAP(0x4F); 	! 4C - 4F undefined
  BAD_TRAP(0x50); BAD_TRAP(0x51); BAD_TRAP(0x52); BAD_TRAP(0x53); 	! 50 - 53 undefined
  BAD_TRAP(0x54); BAD_TRAP(0x55); BAD_TRAP(0x56); BAD_TRAP(0x57); 	! 54 - 57 undefined
  BAD_TRAP(0x58); BAD_TRAP(0x59); BAD_TRAP(0x5A); BAD_TRAP(0x5B); 	! 58 - 5B undefined
  BAD_TRAP(0x5C); BAD_TRAP(0x5D); BAD_TRAP(0x5E); BAD_TRAP(0x5F); 	! 5C - 5F undefined
  BAD_TRAP(0X60); BAD_TRAP(0X61); BAD_TRAP(0X62); BAD_TRAP(0X63); 	! 60 - 63 undefined
  BAD_TRAP(0X64); BAD_TRAP(0X65); BAD_TRAP(0X66); BAD_TRAP(0X67); 	! 64 - 67 undefined
  BAD_TRAP(0X68); BAD_TRAP(0X69); BAD_TRAP(0X6A); BAD_TRAP(0X6B); 	! 68 - 6B undefined
  BAD_TRAP(0X6C); BAD_TRAP(0X6D); BAD_TRAP(0X6E); BAD_TRAP(0X6F); 	! 6C - 6F undefined
  BAD_TRAP(0X70); BAD_TRAP(0X71); BAD_TRAP(0X72); BAD_TRAP(0X73); 	! 70 - 73 undefined
  BAD_TRAP(0X74); BAD_TRAP(0X75); BAD_TRAP(0X76); BAD_TRAP(0X77); 	! 74 - 77 undefined
  BAD_TRAP(0X78); BAD_TRAP(0X79); BAD_TRAP(0X7A); BAD_TRAP(0X7B); 	! 78 - 7B undefined
  BAD_TRAP(0x7C); BAD_TRAP(0x7D); BAD_TRAP(0x7E); BAD_TRAP(0x7F); 	! 7C - 7F undefined

  TRAP( 0x80, syscall ); 			! 80 syscall SW trap
  SOFT_TRAP;  ! 81
  SOFT_TRAP;  ! 82
  TRAP( 0x83, disable_interrupts ); ! 83 enable interrupts
  TRAP( 0x84, enable_interrupts );  ! 84 disable interrupts
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! 85 - 87
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! 88 - 8B
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! 8C - 8F
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! 90 - 93
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! 94 - 97
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! 98 - 9B
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! 9C - 9F
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! A0 - A3
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! A4 - A7
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! A8 - AB
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! AC - AF
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! B0 - B3
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! B4 - B7
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! B8 - BB
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! BC - BF
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! C0 - C3
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! C4 - C7
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! C8 - CB
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! CC - CF
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! D0 - D3
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! D4 - D7
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! D8 - DB
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! DC - DF
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! E0 - E3
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! E4 - E7
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! E8 - EB
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! EC - EF
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! F0 - F3
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! F4 - F7
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! F8 - FB
  SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! FC - FF

	.align 4

system_reset:

	/* Clear the caches */
	flush
	/*	Clear the global registers	*/
	mov	%g0,%g1
	mov	%g0,%g2
	mov	%g0,%g3
	mov	%g0,%g4
	mov	%g0,%g5
	mov	%g0,%g6
	mov	%g0,%g7

	/*	Set the MCTRL registers	*/
!	set	MCTRL, %g3

	/* Memory Config Reg 1 */
!	set	MEMCFG1, %g2
!	st	%g2, [ %g3 ]
	/* Memory Config Reg 2 */
!	set	MEMCFG2, %g2
!	st	%g2, [ %g3 + 0x04 ]
	/* Memory Config Reg 2 */
!	set	MEMCFG3, %g2
!	st	%g2, [ %g3 + 0x08 ]


	set	UARTA, %g3		! Load UARTA scaler
	set	UARTA_SCALER, %g2
  	st	%g2, [%g3 + 0xc]
	set	3, %g2			! Enable UARTA
  	st	%g2, [%g3 + 0x8]
	st	%g0, [%g3 + 0x4]

	mov	%g0, %y	/*	clears the Y register	*/

	mov	8, %g1	/* 8 loops to clear all the LOCAL an IN register windows	*/

	/*	Clear all the IN and LOCAL registers	*/
1:	mov	%g0, %i0
	mov	%g0, %i1
	mov	%g0, %i2
	mov	%g0, %i3
	mov	%g0, %i4
	mov	%g0, %i5
	mov	%g0, %i6
	mov	%g0, %i7
	mov	%g0, %l0
	mov	%g0, %l1
	mov	%g0, %l2
	mov	%g0, %l3
	mov	%g0, %l4
	mov	%g0, %l5
	mov	%g0, %l6
	mov	%g0, %l7

	save
	deccc	%g1
	bne	1b
	nop

	set	trap_table,%g2
	wr	%g2,%tbr		/* TBR at 0x40000000	*/

	nop
	nop
	nop

	/* Set PSR and enable traps	*/

	wr	%g0, PSR_INIT|PSR_ET, %psr

	nop
	nop
	nop

	/* Initialize FPU registers to zero	*/

//#ifndef _SOFT_FLOAT
#ifndef CONFIG_HAS_FPU
	mov	1, %o0
	ld	[ %g0 ], %f0
	tst	%o0
	be	1f
	nop

	ld	[ %g0 ], %f1
	ld	[ %g0 ], %f2
	ld	[ %g0 ], %f3
	ld	[ %g0 ], %f4
	ld	[ %g0 ], %f5
	ld	[ %g0 ], %f6
	ld	[ %g0 ], %f7
	ld	[ %g0 ], %f8
	ld	[ %g0 ], %f9
	ld	[ %g0 ], %f10
	ld	[ %g0 ], %f11
	ld	[ %g0 ], %f12
	ld	[ %g0 ], %f13
	ld	[ %g0 ], %f14
	ld	[ %g0 ], %f15
	ld	[ %g0 ], %f16
	ld	[ %g0 ], %f17
	ld	[ %g0 ], %f18
	ld	[ %g0 ], %f19
	ld	[ %g0 ], %f20
	ld	[ %g0 ], %f21
	ld	[ %g0 ], %f22
	ld	[ %g0 ], %f23
	ld	[ %g0 ], %f24
	ld	[ %g0 ], %f25
	ld	[ %g0 ], %f26
	ld	[ %g0 ], %f27
	ld	[ %g0 ], %f28
	ld	[ %g0 ], %f29
	ld	[ %g0 ], %f30
	ld	[ %g0 ], %f31
#endif

	/*	Initialize the invalid window	*/
1:	wr	%g0, WIM_INIT, %wim
	nop;nop;nop

	/*
	 * Initialize the stack pointer and the frame pointer at the end of the RAM
	 */
//	set SPP_INIT - CPU_MINIMUM_STACK_FRAME_SIZE, %g2
	set SPP_INIT, %g2
	mov %g2, %sp
	mov %g2, %fp

//	call clear_bss
//	nop
	/*
	 * At this point, the ASM initialization is finished so, we call the init
	 * method that initialize the uKernel
	 */

	 call OS_Start
	 sub %sp, CPU_MINIMUM_STACK_FRAME_SIZE, %sp		/*	prepare the stack frame for the init method	*/

fatal_return:
	mov	1, %g1
	ta      0                       ! Halt if _main returns ...
	nop

/*	FPU Interrupt Service Routine	*/

fpu_dis:
    rd %psr, %l0
    wr %l0, PSR_EF, %psr	! Enable the FP unit

    jmpl %l2 - 4, %g0       /*  return to the instruction that provoked the trap, prev instruction   */
    rett %l2
	nop
